// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  

#include "GuPCMTriangleContactGen.h"
#include "GuPCMContactConvexCommon.h"
#include "GuVecTriangle.h"
#include "GuBarycentricCoordinates.h"
#include "GuConvexEdgeFlags.h"
#include "foundation/PxAlloca.h"

#if	PCM_LOW_LEVEL_DEBUG
#include "common/PxRenderBuffer.h"
#endif

#define EDGE_EDGE_GAUSS_MAP		0
#define BRUTE_FORCE_EDGE_EDGE	0

using namespace physx;
using namespace Gu;
using namespace aos;

static bool testPolyFaceNormal(const PolygonalData& polyData, const SupportLocalImpl<TriangleV>* triMap, const SupportLocal* polyMap, const FloatVArg contactDist, 
	FloatV& minOverlap, PxU32& feature, Vec3V& faceNormal, FeatureStatus faceStatus, FeatureStatus& status)
{
	FloatV _minOverlap = FMax();
	PxU32 _feature = 0;
	Vec3V _faceNormal = faceNormal;
	FloatV min0, max0;
	FloatV min1, max1;
	const FloatV eps = FEps();

	if(polyMap->isIdentityScale)
	{
		//in the local space of polyData0
		for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
		{
			const HullPolygonData& polygon = polyData.mPolygons[i];

			const Vec3V minVert = V3LoadU_SafeReadW(polyData.mVerts[polygon.mMinIndex]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
			const FloatV planeDist = FLoad(polygon.mPlane.d);
			//shapeSpace and vertexSpace are the same
			const Vec3V planeNormal = V3LoadU_SafeReadW(polygon.mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
	
			//ML::avoid lHS, don't use the exiting function
			min0 = V3Dot(planeNormal, minVert);
			max0 = FNeg(planeDist);

			triMap->doSupport(planeNormal, min1, max1);

			const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));

			if(BAllEqTTTT(con))
				return false;

			const FloatV tempOverlap = FSub(max0, min1);

			if(FAllGrtr(_minOverlap, tempOverlap))
			{
				_minOverlap = tempOverlap;
				_feature = i;
				_faceNormal = planeNormal;
			}
		}   
	}
	else
	{
		//in the local space of polyData0
		for(PxU32 i=0; i<polyData.mNbPolygons; ++i)
		{
			const HullPolygonData& polygon = polyData.mPolygons[i];

			const Vec3V minVert = V3LoadU_SafeReadW(polyData.mVerts[polygon.mMinIndex]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
			const FloatV planeDist = FLoad(polygon.mPlane.d);
			const Vec3V vertexSpacePlaneNormal = V3LoadU_SafeReadW(polygon.mPlane.n);	// PT: safe because 'd' follows 'n' in the plane class
			//transform plane n to shape space
			const Vec3V shapeSpacePlaneNormal = M33TrnspsMulV3(polyMap->shape2Vertex, vertexSpacePlaneNormal);

			const FloatV magnitude = FRsqrtFast(V3LengthSq(shapeSpacePlaneNormal)); //FRecip(V3Length(shapeSpacePlaneNormal));

			//ML::avoid lHS, don't use the exiting function
			min0 = FMul(V3Dot(vertexSpacePlaneNormal, minVert), magnitude);
			max0 = FMul(FNeg(planeDist), magnitude);

			//normalize the shapeSpacePlaneNormal
			const Vec3V planeN = V3Scale(shapeSpacePlaneNormal, magnitude);

			triMap->doSupport(planeN, min1, max1);

			const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));

			if(BAllEqTTTT(con))
				return false;

			const FloatV tempOverlap = FSub(max0, min1);

			if(FAllGrtr(_minOverlap, tempOverlap))
			{
				_minOverlap = tempOverlap;
				_feature = i;
				_faceNormal = planeN;
			}
		} 
	}

	if(FAllGrtr(minOverlap, FAdd(_minOverlap, eps)))
	{
		faceNormal = _faceNormal;
		minOverlap = _minOverlap;
		status = faceStatus;
	}

	feature = _feature;

	return true;
}

//triangle is in the local space of polyData
static bool testTriangleFaceNormal(const TriangleV& triangle, const PolygonalData& /*polyData*/, const SupportLocalImpl<TriangleV>* /*triMap*/, const SupportLocal* polyMap, const FloatVArg contactDist, 
	FloatV& minOverlap, PxU32& feature, Vec3V& faceNormal, FeatureStatus faceStatus, FeatureStatus& status)
{
	FloatV min1, max1;
	const FloatV eps = FEps();

	const Vec3V triangleLocNormal = triangle.normal();

	const FloatV min0 = V3Dot(triangleLocNormal, triangle.verts[0]);
	const FloatV max0 = min0;

	//triangle normal is in the vertex space
	polyMap->doSupport(triangleLocNormal, min1, max1);

	const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));

	if(BAllEqTTTT(con))
		return false;

	minOverlap = FSub(FSub(max0, min1), eps);
	status = faceStatus;
	feature = 0;
	faceNormal = triangleLocNormal;

	return true;
}

static bool testPolyEdgeNormal(const TriangleV& triangle, const PxU8 triFlags, const PolygonalData& polyData, const SupportLocalImpl<TriangleV>* triMap, const SupportLocal* polyMap, const FloatVArg contactDist,
	FloatV& minOverlap, Vec3V& minNormal, FeatureStatus edgeStatus, FeatureStatus& status)
{
	FloatV overlap = minOverlap;
	FloatV min0, max0;
	FloatV min1, max1;
	const FloatV zero = FZero();
	const Vec3V eps2 = V3Splat(FLoad(1e-6f));
		
	const Vec3V v0 = M33MulV3(polyMap->shape2Vertex, triangle.verts[0]);
	const Vec3V v1 = M33MulV3(polyMap->shape2Vertex, triangle.verts[1]);
	const Vec3V v2 = M33MulV3(polyMap->shape2Vertex, triangle.verts[2]);

	const TriangleV vertexSpaceTriangle(v0, v1, v2);

	PxU32 nbTriangleAxes = 0;
	Vec3V triangleAxes[3];
	for(PxI8 kStart = 0, kEnd =2; kStart<3; kEnd = kStart++)
	{
		const bool active = (triFlags & (1 << (kEnd+3))) != 0;
	
		if(active)
		{
			const Vec3V p00 = vertexSpaceTriangle.verts[kStart];
			const Vec3V p01 = vertexSpaceTriangle.verts[kEnd];
			triangleAxes[nbTriangleAxes++] = V3Sub(p01, p00);
		}
	}

	if(nbTriangleAxes == 0)
		return true;

	//create localTriPlane in the vertex space
	const Vec3V vertexSpaceTriangleNormal = vertexSpaceTriangle.normal();
	
	for(PxU32 i =0; i<polyData.mNbPolygons; ++i)
	{
		const HullPolygonData& polygon = polyData.mPolygons[i];
		const PxU8* inds = polyData.mPolygonVertexRefs + polygon.mVRef8;
		const Vec3V vertexSpacePlaneNormal = V3LoadU(polygon.mPlane.n);

		//fast culling. 
		if(FAllGrtr(V3Dot(vertexSpacePlaneNormal, vertexSpaceTriangleNormal), zero))
			continue;

		// Loop through polygon vertices == polygon edges;
		for(PxU32 lStart = 0, lEnd = PxU32(polygon.mNbVerts-1); lStart<polygon.mNbVerts; lEnd = PxU32(lStart++))
		{
			//in the vertex space
			const Vec3V p10 = V3LoadU_SafeReadW(polyData.mVerts[inds[lStart]]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
			const Vec3V p11 = V3LoadU_SafeReadW(polyData.mVerts[inds[lEnd]]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData

			const Vec3V convexEdge = V3Sub(p11, p10);

			for(PxU32 j = 0; j < nbTriangleAxes; ++j)
			{
				const Vec3V currentPolyEdge = triangleAxes[j];
				const Vec3V v = V3Cross(convexEdge, currentPolyEdge);

				//two edges aren't parallel
				if((!V3AllGrtr(eps2, V3Abs(v))) && (FAllGrtr(V3Dot(v, vertexSpaceTriangleNormal), zero)))
				{
					//transform the v back to the shape space
					const Vec3V shapeSpaceV = M33TrnspsMulV3(polyMap->shape2Vertex, v);
					const Vec3V n0 = V3Normalize(shapeSpaceV);
					triMap->doSupport(n0, min0, max0);
					polyMap->doSupport(n0, min1, max1);
					const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
					if(BAllEqTTTT(con))
						return false;

					const FloatV tempOverlap0 = FSub(max0, min1);
					const FloatV tempOverlap1 = FSub(max1, min0);

					if(FAllGrtr(overlap, tempOverlap0))
					{
						overlap = tempOverlap0;
						minNormal = n0;
						status = edgeStatus;
					}

					if(FAllGrtr(overlap, tempOverlap1))
					{
						overlap = tempOverlap1;
						minNormal = V3Neg(n0);
						status = edgeStatus;
					}
				}
			}
		}
	}
	minOverlap = overlap;
		
	return true;
}

#if BRUTE_FORCE_EDGE_EDGE
static bool testPolyEdgeNormalBruteForce(const TriangleV& triangle, PxU8 triFlags, const PolygonalData& polyData, const SupportLocalImpl<TriangleV>* triMap, const SupportLocal* polyMap, const FloatVArg contactDist,
	FloatV& minOverlap, Vec3V& minNormal, FeatureStatus edgeStatus, FeatureStatus& status)
{
	PX_UNUSED(triFlags);
	FloatV min0, max0;
	FloatV min1, max1;

	FloatV bestDist = FLoad(PX_MAX_F32);
	Vec3V bestAxis = V3Zero();

	const Vec3V eps2 = V3Splat(FLoad(1e-6));

	PxU32 bestPolyIndex = 0;
	PxU32 bestStart = 0;
	PxU32 bestEnd = 0;
	PxI8 bestTriStart = 0;
	PxI8 bestTriEnd = 0;

	for(PxU32 i =0; i<polyData.mNbPolygons; ++i)
	{
		const HullPolygonData& polygon = polyData.mPolygons[i];
		const PxU8* inds = polyData.mPolygonVertexRefs + polygon.mVRef8;
			
		// Loop through polygon vertices == polygon edges;
		for(PxU32 lStart = 0, lEnd = PxU32(polygon.mNbVerts-1); lStart<polygon.mNbVerts; lEnd = PxU32(lStart++))
		{
			//in the vertex space
			const Vec3V p10 = V3LoadU_SafeReadW(polyData.mVerts[inds[lStart]]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData
			const Vec3V p11 = V3LoadU_SafeReadW(polyData.mVerts[inds[lEnd]]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData

			//shape sapce
			const Vec3V vertex10 = M33MulV3(polyMap->vertex2Shape, p10);
			const Vec3V vertex11 = M33MulV3(polyMap->vertex2Shape, p11);

			const Vec3V convexEdge = V3Sub(vertex11, vertex10);

			for (PxI8 kEnd = 0, kStart = 2; kEnd<3; kStart = kEnd++)
			{
				const Vec3V triVert0 = triangle.verts[kStart];
				const Vec3V triVert1 = triangle.verts[kEnd];

				const Vec3V triangleEdge = V3Sub(triVert1, triVert0);
				const Vec3V v = V3Cross(convexEdge, triangleEdge);

				if (!V3AllGrtr(eps2, V3Abs(v)))
				{
					//transform the v back to the shape space
					const Vec3V n0 = V3Normalize(v);
					triMap->doSupport(n0, min0, max0);
					polyMap->doSupport(n0, min1, max1);
					const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
					if(BAllEqTTTT(con))
						return false;

					const FloatV tempOverlap = FSub(max0, min1);

					if (FAllGrtr(bestDist, tempOverlap))
					{
						bestDist = tempOverlap;
						bestAxis = n0;
						bestPolyIndex = i;
						bestStart = lStart;
						bestEnd = lEnd;
						bestTriStart = kStart;
						bestTriEnd = kEnd;
					}
				}
			}
		}
	}

	if (FAllGrtr(minOverlap, bestDist))
	{
		minOverlap = bestDist;
		minNormal = bestAxis;
		status = edgeStatus;
	}

	return true;
}

static bool testPolyEdgeNormalBruteForceVertsByEdges(const TriangleV& triangle, PxU8 triFlags, const PolygonalData& polyData, const SupportLocalImpl<TriangleV>* triMap, const SupportLocal* polyMap, const FloatVArg contactDist,
	FloatV& minOverlap, Vec3V& minNormal, FeatureStatus edgeStatus, FeatureStatus& status, PxU32& bestEdgeIndex, PxI8& bestTriStart, PxI8& bestTriEnd)
{
	PX_UNUSED(triFlags);

	const PxU32 numConvexEdges = polyData.mNbEdges;
	const PxU16* verticesByEdges16 = polyData.mVerticesByEdges;
	const PxVec3* vertices = polyData.mVerts;

	FloatV bestDist = FLoad(PX_MAX_F32);
	Vec3V bestAxis = V3Zero();
	const Vec3V eps2 = V3Splat(FLoad(1e-6));

	for (PxU32 convexEdgeIdx = 0; convexEdgeIdx < numConvexEdges; ++convexEdgeIdx)
	{
		const PxU16 v0idx1 = verticesByEdges16[convexEdgeIdx * 2];
		const PxU32 v1idx1 = verticesByEdges16[convexEdgeIdx * 2 + 1];

		//shape sapce
		const Vec3V vertex10 = M33MulV3(polyMap->vertex2Shape, V3LoadU(vertices[v0idx1]));
		const Vec3V vertex11 = M33MulV3(polyMap->vertex2Shape, V3LoadU(vertices[v1idx1]));

		Vec3V convexEdge = V3Sub(vertex11, vertex10);

		for (PxI8 kEnd = 0, kStart = 2; kEnd<3; kStart = kEnd++)
		{
			const Vec3V triVert0 = triangle.verts[kStart];
			const Vec3V triVert1 = triangle.verts[kEnd];
			const Vec3V triEdge = V3Sub(triVert1, triVert0);

			// compute the separation along this axis in vertex space
			Vec3V axis = V3Cross(convexEdge, triEdge);

			if (!V3AllGrtr(eps2, V3Abs(axis)))
			{
				axis = V3Normalize(axis);

				FloatV min0, max0;
				FloatV min1, max1;
				triMap->doSupport(axis, min0, max0);
				polyMap->doSupport(axis, min1, max1);
				const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
				if (BAllEqTTTT(con))
					return false;

				const FloatV dist0 = FSub(max0, min1);
				const FloatV dist1 = FSub(max1, min0);

				if (FAllGrtr(dist0, dist1))
					axis = V3Neg(axis);

				const FloatV dist = FMin(dist0, dist1);

				if (FAllGrtr(bestDist, dist))
				{
					bestDist = dist;
					bestAxis = axis;

					bestEdgeIndex = convexEdgeIdx;
					bestTriStart = kStart;
					bestTriEnd = kEnd;
				}
			}
		}
	}

	if (FAllGrtr(minOverlap, bestDist))
	{
		minOverlap = bestDist;
		minNormal = bestAxis;
		status = edgeStatus;

	}
	return true;
}
#endif

#if EDGE_EDGE_GAUSS_MAP
static bool isMinkowskiFace(const Vec3V& A, const Vec3V& B, const Vec3V& B_x_A, const Vec3V& C, const Vec3V& D, const Vec3V& D_x_C)
{
	const FloatV zero = FZero();
	// Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map. 
	// The associated arcs are defined by the adjacent face normals of each edge.  
	const FloatV CBA = V3Dot(C, B_x_A);
	const FloatV DBA = V3Dot(D, B_x_A);
	const FloatV ADC = V3Dot(A, D_x_C);
	const FloatV BDC = V3Dot(B, D_x_C);

	const BoolV con0 = FIsGrtrOrEq(zero, FMul(CBA, DBA));
	const BoolV con1 = FIsGrtrOrEq(zero, FMul(ADC, BDC));
	const BoolV con2 = FIsGrtr(FMul(CBA, BDC), zero);

	const BoolV con = BAnd(con2, BAnd(con0, con1));

	return (BAllEqTTTT(con) != 0);

	//return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f;
}

#define SAT_VARIFY	0
static bool testPolyEdgeNormalGaussMap(const TriangleV& triangle, PxU8 /*triFlags*/, const PolygonalData& polyData, const SupportLocalImpl<TriangleV>* /*triMap*/, const SupportLocal* polyMap, const FloatVArg contactDist,
	FloatV& minOverlap, Vec3V& minNormal, FeatureStatus edgeStatus, FeatureStatus& status, PxU32& gaussMapBestEdgeIndex, PxI8& gaussMapBestTriStart, PxI8& gaussMapBestTriEnd)
{
	const FloatV zero = FZero();

	const Vec3V triNormal = triangle.normal();

	const PxU32 numConvexEdges = polyData.mNbEdges;
	const PxU16* verticesByEdges16 = polyData.mVerticesByEdges;
	const PxU8* facesByEdges8 = polyData.mFacesByEdges;
	const PxVec3* vertices = polyData.mVerts;

	FloatV bestDist = FLoad(-PX_MAX_F32);
	Vec3V axis = V3Zero();
	Vec3V bestAxis = V3Zero();
	const Vec3V eps2 = V3Splat(FLoad(1e-6));

	//Center is in shape space
	const Vec3V shapeSpaceCOM = V3LoadU(polyData.mCenter);
	const Vec3V vertexSpaceCOM = M33MulV3(polyMap->shape2Vertex, shapeSpaceCOM);

	PxVec3 vertexCOM;
	V3StoreU(vertexSpaceCOM, vertexCOM);

	for (PxU32 convexEdgeIdx = 0; convexEdgeIdx < numConvexEdges; ++convexEdgeIdx)
	{
		const PxU16 v0idx1 = verticesByEdges16[convexEdgeIdx*2];
		const PxU32 v1idx1 = verticesByEdges16[convexEdgeIdx * 2 + 1];

		const PxU8 f10 = facesByEdges8[convexEdgeIdx * 2];
		const PxU8 f11 = facesByEdges8[convexEdgeIdx * 2 + 1];

		//shape sapce
		const Vec3V vertex10 = M33MulV3(polyMap->vertex2Shape, V3LoadU(vertices[v0idx1]));
		const Vec3V vertex11 = M33MulV3(polyMap->vertex2Shape, V3LoadU(vertices[v1idx1]));

		Vec3V convexEdge = V3Sub(vertex11, vertex10);

		const HullPolygonData& face0 = polyData.mPolygons[f10];
		const HullPolygonData& face1 = polyData.mPolygons[f11];

		const Vec3V convexNormal0 = M33TrnspsMulV3(polyMap->shape2Vertex, V3LoadU(face0.mPlane.n));
		const Vec3V convexNormal1 = M33TrnspsMulV3(polyMap->shape2Vertex, V3LoadU(face1.mPlane.n));

		float signDist0 = face0.mPlane.distance(vertexCOM);
		float signDist1 = face1.mPlane.distance(vertexCOM);

		PX_ASSERT(signDist0 < 0.f);
		PX_ASSERT(signDist1 < 0.f);

		for (PxI8 kEnd = 0, kStart = 2; kEnd<3; kStart = kEnd++)
		{
			const Vec3V triVert0 = triangle.verts[kStart];
			const Vec3V triVert1 = triangle.verts[kEnd];
			const Vec3V triEdge = V3Sub(triVert1, triVert0);

			//if (isMinkowskiFace(convexNormal0, convexNormal1, V3Neg(convexEdge), V3Neg(triNormal), triNormal, V3Neg(triEdge)))
			if (isMinkowskiFace(convexNormal0, convexNormal1, convexEdge, V3Neg(triNormal), triNormal, triEdge))
			{
				// compute the separation along this axis in vertex space
				axis = V3Cross(convexEdge, triEdge);

				if (!V3AllGrtr(eps2, V3Abs(axis)))
				{
					axis = V3Normalize(axis);

					const Vec3V v = V3Sub(vertex10, shapeSpaceCOM);

					// ensure the axis is outward pointing on the edge on the minkowski sum. Assure normal points from convex hull to triangle
					const FloatV temp = V3Dot(axis, v);
					if (FAllGrtr(zero, temp))
						axis = V3Neg(axis);

					//compute the distance from any of the verts in the triangle to plane(axis, vertex10)(n.dot(p-a))
					const Vec3V ap = V3Sub(triVert0, vertex10);

					const FloatV dist = V3Dot(axis, ap);

					if (FAllGrtr(dist, contactDist))
						return false;
#if SAT_VARIFY
					FloatV min0, max0;
					FloatV min1, max1;
					triMap->doSupport(V3Neg(axis), min0, max0);
					polyMap->doSupport(V3Neg(axis), min1, max1);
					const BoolV con = BOr(FIsGrtr(min1, FAdd(max0, contactDist)), FIsGrtr(min0, FAdd(max1, contactDist)));
					if (BAllEqTTTT(con))
						return false;

					/*const FloatV dist = FSub(max0, min1);

					if (FAllGrtr(bestDist, dist))
					{
					bestDist = dist;
					bestAxis = axis;
					}*/
					const FloatV tempDist = FSub(max0, min1);

					const FloatV dif = FAbs(FAdd(tempDist, dist));
					PX_UNUSED(dif);
					PX_ASSERT(FAllGrtr(FLoad(1e-4f), dif));
#endif
					if (FAllGrtr(dist, bestDist))
					{
						bestDist = dist;
						bestAxis = axis;

						gaussMapBestEdgeIndex = convexEdgeIdx;
						gaussMapBestTriStart = kStart;
						gaussMapBestTriEnd = kEnd;
					}
				}
			}
		}
	}

	if (FAllGrtr(minOverlap, bestDist))
	{
		minOverlap = bestDist;
		minNormal = bestAxis;
		status = edgeStatus;
	}
	return true;
}
#endif

static PX_FORCE_INLINE PxU32 addMeshContacts(MeshPersistentContact* manifoldContacts, const Vec3V& pA, const Vec3V& pB, const Vec4V& normalPen, PxU32 triangleIndex, PxU32 numContacts)
{
	manifoldContacts[numContacts].mLocalPointA = pA;
	manifoldContacts[numContacts].mLocalPointB = pB;
	manifoldContacts[numContacts].mLocalNormalPen = normalPen;
	manifoldContacts[numContacts].mFaceIndex = triangleIndex;
	return numContacts+1;
}

static void generatedTriangleContacts(const TriangleV& triangle, PxU32 triangleIndex, PxU8/* _triFlags*/, const PolygonalData& polyData1, const HullPolygonData& incidentPolygon, const SupportLocal* map1, MeshPersistentContact* manifoldContacts, PxU32& numManifoldContacts, 
	const FloatVArg contactDist, const Vec3VArg contactNormal, PxRenderOutput* renderOutput)
{
	PX_UNUSED(renderOutput);

	//PxU8 triFlags = _triFlags;
	const PxU32 previousContacts = numManifoldContacts;
		
	const FloatV zero = FZero();
	
	const Mat33V rot = findRotationMatrixFromZAxis(contactNormal);

	const PxU8* inds1 = polyData1.mPolygonVertexRefs + incidentPolygon.mVRef8;

	Vec3V points0In0[3];
	Vec3V* points1In0 = reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*incidentPolygon.mNbVerts, 16));
	FloatV* points1In0TValue = reinterpret_cast<FloatV*>(PxAllocaAligned(sizeof(FloatV)*incidentPolygon.mNbVerts, 16));
	bool* points1In0Penetration = reinterpret_cast<bool*>(PxAlloca(sizeof(bool)*incidentPolygon.mNbVerts));

	points0In0[0] = triangle.verts[0];
	points0In0[1] = triangle.verts[1];
	points0In0[2] = triangle.verts[2];

	//Transform all the verts from vertex space to shape space
	map1->populateVerts(inds1, incidentPolygon.mNbVerts, polyData1.mVerts, points1In0);

#if PCM_LOW_LEVEL_DEBUG
	PersistentContactManifold::drawPolygon(*renderOutput, map1->transform, points1In0, incidentPolygon.mNbVerts, (PxU32)PxDebugColor::eARGB_RED);
	//PersistentContactManifold::drawTriangle(*renderOutput, map1->transform.transform(points1In0[0]), map1->transform.transform(points0In0[1]), map1->transform.transform(points0In0[2]), (PxU32)PxDebugColor::eARGB_BLUE);
#endif
 
	Vec3V eps = Vec3V_From_FloatV(FEps());
	Vec3V max = Vec3V_From_FloatV(FMax());
	Vec3V nmax = V3Neg(max); 

	//transform reference polygon to 2d, calculate min and max
	Vec3V rPolygonMin = max;
	Vec3V rPolygonMax = nmax;
	for(PxU32 i=0; i<3; ++i)
	{
		points0In0[i] = M33MulV3(rot, points0In0[i]);
		rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
		rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
	}
		
	rPolygonMin = V3Sub(rPolygonMin, eps);
	rPolygonMax = V3Add(rPolygonMax, eps);

	const FloatV d = V3GetZ(points0In0[0]);
	const FloatV rd = FAdd(d, contactDist);

	Vec3V iPolygonMin = max; 
	Vec3V iPolygonMax = nmax;

	PxU32 inside = 0;
	for(PxU32 i=0; i<incidentPolygon.mNbVerts; ++i)
	{
		const Vec3V vert1 = points1In0[i]; //this still in polyData1's local space
		points1In0[i] = M33MulV3(rot, vert1);
		const FloatV z = V3GetZ(points1In0[i]);
		points1In0TValue[i] = FSub(z, d);
		points1In0[i] = V3SetZ(points1In0[i], d);
		iPolygonMin = V3Min(iPolygonMin, points1In0[i]);
		iPolygonMax = V3Max(iPolygonMax, points1In0[i]);

		bool penetrated = false;
			
		if(FAllGrtr(rd, z))
		{
			penetrated = true;

			if(contains(points0In0, 3, points1In0[i], rPolygonMin, rPolygonMax))
			{
				inside++;

				//add this contact to the buffer
				const FloatV t = V3Dot(contactNormal, V3Sub(triangle.verts[0], vert1));
				const Vec3V projectPoint = V3ScaleAdd(contactNormal, t, vert1);
				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), FNeg(t));
				numManifoldContacts = addMeshContacts(manifoldContacts, vert1, projectPoint, localNormalPen, triangleIndex, numManifoldContacts);

				//if the numContacts are more than GU_MESH_CONTACT_REDUCTION_THRESHOLD, we need to do contact reduction
				const PxU32 numContacts = numManifoldContacts - previousContacts;
				if(numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
				{
					//a polygon has more than GU_MESH_CONTACT_REDUCTION_THRESHOLD(16) contacts with this triangle, we will reduce
					//the contacts to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(6) points
					SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
					numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
				}
			}
		}

		points1In0Penetration[i] = penetrated;
	}

	if(inside == incidentPolygon.mNbVerts)
		return;

	inside = 0;
	iPolygonMin = V3Sub(iPolygonMin, eps);
	iPolygonMax = V3Add(iPolygonMax, eps);

	const Vec3V incidentNormal = V3Normalize(M33TrnspsMulV3(map1->shape2Vertex, V3LoadU(incidentPolygon.mPlane.n)));
	const FloatV iPlaneD = V3Dot(incidentNormal, M33MulV3(map1->vertex2Shape, V3LoadU(polyData1.mVerts[inds1[0]])));

	for(PxU32 i=0; i<3; ++i)
	{
		if(contains(points1In0, incidentPolygon.mNbVerts, points0In0[i], iPolygonMin, iPolygonMax))
		{
			inside++;

			const Vec3V vert0 = M33TrnspsMulV3(rot, points0In0[i]);
			const FloatV t = FSub(V3Dot(incidentNormal, vert0), iPlaneD);

			if(FAllGrtr(t, contactDist))
				continue;

			const Vec3V projPoint = V3NegScaleSub(incidentNormal, t, vert0);

			const Vec3V v = V3Sub(projPoint, vert0);
			const FloatV t3 = V3Dot(v, contactNormal);

			const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), t3);
			numManifoldContacts = addMeshContacts(manifoldContacts, projPoint, vert0, localNormalPen, triangleIndex, numManifoldContacts);

			//if the numContacts are more than GU_MESH_CONTACT_REDUCTION_THRESHOLD, we need to do contact reduction
			const PxU32 numContacts = numManifoldContacts - previousContacts;
			if(numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
			{
				//a polygon has more than GU_MESH_CONTACT_REDUCTION_THRESHOLD(16) contacts with this triangle, we will reduce
				//the contacts to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(4) points
				SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
				numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
			}
		}
	}

	if(inside == 3)
		return;  
	
	//(2) segment intersection
	for(PxU32 rStart = 0, rEnd = 2; rStart < 3; rEnd = rStart++)
	{
		const Vec3V rpA = points0In0[rStart];
		const Vec3V rpB = points0In0[rEnd];

		const Vec3V rMin = V3Min(rpA, rpB);
		const Vec3V rMax = V3Max(rpA, rpB);

		for(PxU32 iStart = 0, iEnd = PxU32(incidentPolygon.mNbVerts - 1); iStart < incidentPolygon.mNbVerts; iEnd = iStart++)
		{
			if((!points1In0Penetration[iStart] && !points1In0Penetration[iEnd]))//|| (points1In0[i].status == POINT_OUTSIDE && points1In0[incidentIndex].status == POINT_OUTSIDE))
				continue;

			const Vec3V ipA = points1In0[iStart];
			const Vec3V ipB = points1In0[iEnd];

			const Vec3V iMin = V3Min(ipA, ipB);
			const Vec3V iMax = V3Max(ipA, ipB);

			const BoolV tempCon = BOr(V3IsGrtr(iMin, rMax), V3IsGrtr(rMin, iMax));
			const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));

			if(BAllEqTTTT(con))
				continue;

			const FloatV a1 = signed2DTriArea(rpA, rpB, ipA);
			const FloatV a2 = signed2DTriArea(rpA, rpB, ipB);

			if(FAllGrtr(zero, FMul(a1, a2)))
			{
				const FloatV a3 = signed2DTriArea(ipA, ipB, rpA);
				const FloatV a4 = signed2DTriArea(ipA, ipB, rpB);

				if(FAllGrtr(zero, FMul(a3, a4)))
				{
					//these two segment intersect in 2d
					const FloatV t = FMul(a1, FRecip(FSub(a2, a1)));

					const Vec3V ipAOri = V3SetZ(points1In0[iStart], FAdd(points1In0TValue[iStart], d));
					const Vec3V ipBOri = V3SetZ(points1In0[iEnd], FAdd(points1In0TValue[iEnd], d));

					const Vec3V pBB = V3NegScaleSub(V3Sub(ipBOri, ipAOri), t, ipAOri);
					const Vec3V pAA = V3SetZ(pBB, d);
					const Vec3V pA = M33TrnspsMulV3(rot, pAA);
					const Vec3V pB = M33TrnspsMulV3(rot, pBB);
					const FloatV pen = FSub(V3GetZ(pBB), V3GetZ(pAA));

					if(FAllGrtr(pen, contactDist))
						continue;

					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(contactNormal), pen);
					numManifoldContacts = addMeshContacts(manifoldContacts, pB, pA, localNormalPen, triangleIndex, numManifoldContacts);

					//if the numContacts are more than GU_MESH_CONTACT_REDUCTION_THRESHOLD, we need to do contact reduction
					const PxU32 numContacts = numManifoldContacts - previousContacts;
					if(numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
					{
						//a polygon has more than GU_MESH_CONTACT_REDUCTION_THRESHOLD(16) contacts with this triangle, we will reduce
						//the contacts to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(4) points
						SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
						numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
					}
				}
			}
		}
	}
}

static void generatedPolyContacts(const PolygonalData& polyData0, const HullPolygonData& referencePolygon, const TriangleV& triangle, PxU32 triangleIndex, PxU8 triFlags, 
	const SupportLocal* map0, MeshPersistentContact* manifoldContacts, PxU32& numManifoldContacts, const FloatVArg contactDist, const Vec3VArg contactNormal, PxRenderOutput* renderOutput)
{
	PX_UNUSED(triFlags);
	PX_UNUSED(renderOutput);

	const FloatV zero = FZero();

	const PxU32 previousContacts = numManifoldContacts;

	const PxU8* inds0 = polyData0.mPolygonVertexRefs + referencePolygon.mVRef8;

	const Vec3V nContactNormal = V3Neg(contactNormal);

	//this is the matrix transform all points to the 2d plane
	const Mat33V rot = findRotationMatrixFromZAxis(contactNormal);

	Vec3V* points0In0=reinterpret_cast<Vec3V*>(PxAllocaAligned(sizeof(Vec3V)*referencePolygon.mNbVerts, 16));
	Vec3V points1In0[3];
	FloatV points1In0TValue[3];

	bool points1In0Penetration[3] = { false, false, false };
		
	//Transform all the verts from vertex space to shape space
	map0->populateVerts(inds0, referencePolygon.mNbVerts, polyData0.mVerts, points0In0);

	points1In0[0] = triangle.verts[0];
	points1In0[1] = triangle.verts[1];
	points1In0[2] = triangle.verts[2];

#if PCM_LOW_LEVEL_DEBUG
	PersistentContactManifold::drawPolygon(*renderOutput, map0->transform, points0In0, referencePolygon.mNbVerts, (PxU32)PxDebugColor::eARGB_GREEN);
	//PersistentContactManifold::drawTriangle(*gRenderOutPut, map0->transform.transform(points1In0[0]), map0->transform.transform(points1In0[1]), map0->transform.transform(points1In0[2]), (PxU32)PxDebugColor::eARGB_BLUE);
#endif

	//the first point in the reference plane
	const Vec3V referencePoint = points0In0[0];
 
	Vec3V eps = Vec3V_From_FloatV(FEps());
	Vec3V max = Vec3V_From_FloatV(FMax());
	Vec3V nmax = V3Neg(max); 

	//transform reference polygon to 2d, calculate min and max
	Vec3V rPolygonMin = max;
	Vec3V rPolygonMax = nmax;
	for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
	{
		//points0In0[i].vertext = M33TrnspsMulV3(rot, Vec3V_From_PxVec3(polyData0.mVerts[inds0[i]]));
		points0In0[i] = M33MulV3(rot, points0In0[i]);
		rPolygonMin = V3Min(rPolygonMin, points0In0[i]);
		rPolygonMax = V3Max(rPolygonMax, points0In0[i]);
	}
		
	rPolygonMin = V3Sub(rPolygonMin, eps);
	rPolygonMax = V3Add(rPolygonMax, eps);
		
	const FloatV d = V3GetZ(points0In0[0]);

	const FloatV rd = FAdd(d, contactDist);

	Vec3V iPolygonMin = max; 
	Vec3V iPolygonMax = nmax;

	PxU32 inside = 0;
	for(PxU32 i=0; i<3; ++i)
	{
		const Vec3V vert1 = points1In0[i]; //this still in polyData1's local space
		points1In0[i] = M33MulV3(rot, vert1);
		const FloatV z = V3GetZ(points1In0[i]);
		points1In0TValue[i] = FSub(z, d);
		points1In0[i] = V3SetZ(points1In0[i], d);
		iPolygonMin = V3Min(iPolygonMin, points1In0[i]);
		iPolygonMax = V3Max(iPolygonMax, points1In0[i]);
		if(FAllGrtr(rd, z))
		{
			points1In0Penetration[i] = true;

			//ML : check to see whether all the points of triangles in 2D space are within reference polygon's range
			if(contains(points0In0, referencePolygon.mNbVerts, points1In0[i], rPolygonMin, rPolygonMax))
			{
				inside++;

				//calculate projection point
				const FloatV t = V3Dot(contactNormal, V3Sub(vert1, referencePoint));
				const Vec3V projectPoint = V3NegScaleSub(contactNormal, t, vert1);
					
				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(nContactNormal), t);
				numManifoldContacts = addMeshContacts(manifoldContacts, projectPoint, vert1, localNormalPen, triangleIndex, numManifoldContacts);

				//if the numContacts are more than GU_MESH_CONTACT_REDUCTION_THRESHOLD, we need to do contact reduction
				const PxU32 numContacts = numManifoldContacts - previousContacts;
				if(numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
				{
					//a polygon has more than GU_MESH_CONTACT_REDUCTION_THRESHOLD(16) contacts with this triangle, we will reduce
					//the contacts to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(4) points
					SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
					numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
				}
			}
		}
	}

	if(inside == 3)
		return;

	inside = 0;
	iPolygonMin = V3Sub(iPolygonMin, eps);
	iPolygonMax = V3Add(iPolygonMax, eps);

	const Vec3V incidentNormal = triangle.normal();
	const FloatV iPlaneD = V3Dot(incidentNormal, triangle.verts[0]);
	const FloatV one = FOne();
	for(PxU32 i=0; i<referencePolygon.mNbVerts; ++i)
	{
		if(contains(points1In0, 3, points0In0[i], iPolygonMin, iPolygonMax))
		{
			const Vec3V vert0 = M33TrnspsMulV3(rot, points0In0[i]);

			const FloatV t = FSub(V3Dot(incidentNormal, vert0), iPlaneD);
				
			if(FAllGrtr(t, contactDist))
				continue;

			const Vec3V projPoint = V3NegScaleSub(incidentNormal, t, vert0);

			FloatV u, w;
			barycentricCoordinates(projPoint, triangle.verts[0], triangle.verts[1], triangle.verts[2], u, w);
			const BoolV con = BAnd(FIsGrtrOrEq(u, zero), BAnd(FIsGrtrOrEq(w, zero), FIsGrtrOrEq(one, FAdd(u, w))));
			
			if(BAllEqTTTT(con))
			{
				inside++;

				const Vec3V v = V3Sub(projPoint, vert0);
				const FloatV t3 = V3Dot(v, contactNormal);
					
				const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(nContactNormal), t3);
				numManifoldContacts = addMeshContacts(manifoldContacts, vert0, projPoint, localNormalPen, triangleIndex, numManifoldContacts);
				
				//if the numContacts are more than GU_MESH_CONTACT_REDUCTION_THRESHOLD, we need to do contact reduction
				const PxU32 numContacts = numManifoldContacts - previousContacts;
				if(numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
				{
					//a polygon has more than GU_MESH_CONTACT_REDUCTION_THRESHOLD(16) contacts with this triangle, we will reduce
					//the contacts to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(4) points
					SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
					numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
				}
			}
		}
	}

	if(inside == referencePolygon.mNbVerts)
		return;
	
	//Always generate segment contacts
	//(2) segment intersection
	for(PxU32 iStart = 0, iEnd = 2; iStart < 3; iEnd = iStart++)
	{
		if((!points1In0Penetration[iStart] && !points1In0Penetration[iEnd] ) )
			continue;
			
		const Vec3V ipA = points1In0[iStart];
		const Vec3V ipB = points1In0[iEnd];

		const Vec3V iMin = V3Min(ipA, ipB);
		const Vec3V iMax = V3Max(ipA, ipB);
		
		for(PxU32 rStart = 0, rEnd = PxU32(referencePolygon.mNbVerts - 1); rStart < referencePolygon.mNbVerts; rEnd = rStart++) 
		{
			const Vec3V rpA = points0In0[rStart];
			const Vec3V rpB = points0In0[rEnd];

			const Vec3V rMin = V3Min(rpA, rpB);
			const Vec3V rMax = V3Max(rpA, rpB);
				
			const BoolV tempCon = BOr(V3IsGrtr(iMin, rMax), V3IsGrtr(rMin, iMax));
			const BoolV con = BOr(BGetX(tempCon), BGetY(tempCon));
		
			if(BAllEqTTTT(con))
				continue;
					
			const FloatV a1 = signed2DTriArea(rpA, rpB, ipA);
			const FloatV a2 = signed2DTriArea(rpA, rpB, ipB);

			if(FAllGrtr(zero, FMul(a1, a2)))
			{
				const FloatV a3 = signed2DTriArea(ipA, ipB, rpA);
				const FloatV a4 = signed2DTriArea(ipA, ipB, rpB);

				if(FAllGrtr(zero, FMul(a3, a4)))
				{
					//these two segment intersect
					const FloatV t = FMul(a1, FRecip(FSub(a2, a1)));

					const Vec3V ipAOri = V3SetZ(points1In0[iStart], FAdd(points1In0TValue[iStart], d));
					const Vec3V ipBOri = V3SetZ(points1In0[iEnd], FAdd(points1In0TValue[iEnd], d));

					const Vec3V pBB = V3NegScaleSub(V3Sub(ipBOri, ipAOri), t, ipAOri); 
					const Vec3V pAA = V3SetZ(pBB, d);
					const Vec3V pA = M33TrnspsMulV3(rot, pAA);
					const Vec3V pB = M33TrnspsMulV3(rot, pBB);
					const FloatV pen = FSub(V3GetZ(pBB), V3GetZ(pAA));
				
					if(FAllGrtr(pen, contactDist))
						continue;
					
					const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(nContactNormal), pen);
					numManifoldContacts = addMeshContacts(manifoldContacts, pA, pB, localNormalPen, triangleIndex, numManifoldContacts);
						
					//if the numContacts are more than GU_MESH_CONTACT_REDUCTION_THRESHOLD, we need to do contact reduction
					const PxU32 numContacts = numManifoldContacts - previousContacts;
					if(numContacts >= GU_MESH_CONTACT_REDUCTION_THRESHOLD)
					{
						//a polygon has more than GU_MESH_CONTACT_REDUCTION_THRESHOLD(16) contacts with this triangle, we will reduce
						//the contacts to GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE(4) points
						SinglePersistentContactManifold::reduceContacts(&manifoldContacts[previousContacts], numContacts);
						numManifoldContacts = previousContacts + GU_SINGLE_MANIFOLD_SINGLE_POLYGONE_CACHE_SIZE;
					}
				}
			}
		}
	}
}

bool Gu::PCMConvexVsMeshContactGeneration::generateTriangleFullContactManifold(const TriangleV& localTriangle, PxU32 triangleIndex, const PxU32* triIndices, PxU8 triFlags, const PolygonalData& polyData, const SupportLocalImpl<TriangleV>* localTriMap, const SupportLocal* polyMap, MeshPersistentContact* manifoldContacts, PxU32& numContacts,
	const FloatVArg contactDist, Vec3V& patchNormal)
{
	FeatureStatus status = POLYDATA0;
	FloatV minOverlap = FMax();
	//minNormal will be in the local space of polyData
	Vec3V minNormal = V3Zero();

	PxU32 feature0;
	if(!testTriangleFaceNormal(localTriangle, polyData, localTriMap, polyMap, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
		return false;

	PxU32 feature1;
	if(!testPolyFaceNormal(polyData, localTriMap, polyMap, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
		return false;

	if(!testPolyEdgeNormal(localTriangle, triFlags, polyData, localTriMap, polyMap, contactDist, minOverlap, minNormal, EDGE, status))
		return false;

	const Vec3V triNormal = localTriangle.normal();

	if(status == POLYDATA0)
	{
		//minNormal is the triangle normal and it is in the local space of polydata0

		PxI32 index2;
		PxI32 polyIndex = getPolygonIndex(polyData, polyMap, minNormal, index2);
		const HullPolygonData& referencePolygon = polyData.mPolygons[polyIndex];

		patchNormal = triNormal;
		generatedTriangleContacts(localTriangle, triangleIndex, triFlags, polyData, referencePolygon, polyMap, manifoldContacts, numContacts, contactDist, triNormal, mRenderOutput);

		if(index2 != -1)
		{
			generatedTriangleContacts(localTriangle, triangleIndex, triFlags, polyData, polyData.mPolygons[index2], polyMap, manifoldContacts, numContacts, contactDist, triNormal, mRenderOutput);
		}
	}
	else
	{
		if(status == POLYDATA1)
		{
			const HullPolygonData* referencePolygon = &polyData.mPolygons[feature1];
			
			const FloatV cosTheta = V3Dot(V3Neg(minNormal), triNormal);
					
			const FloatV threshold = FLoad(0.707106781f);//about 45 degree0

			if(FAllGrtr(cosTheta, threshold))
			{
				patchNormal = triNormal;
				generatedTriangleContacts(localTriangle, triangleIndex, triFlags, polyData, *referencePolygon, polyMap, manifoldContacts, numContacts, contactDist, triNormal, mRenderOutput);
			}
			else
			{
				//ML : defer the contacts generation

				if(mSilhouetteEdgesAreActive ||
					!(triFlags & (ETD_SILHOUETTE_EDGE_01 | ETD_SILHOUETTE_EDGE_12 | ETD_SILHOUETTE_EDGE_20)))
				{
					const PxU32 nb = sizeof(PCMDeferredPolyData) / sizeof(PxU32);
					PxU32 newSize = nb + mDeferredContacts->size();
					if(mDeferredContacts->capacity() < newSize)
						mDeferredContacts->reserve((newSize+1)*2);
					PCMDeferredPolyData* PX_RESTRICT data = reinterpret_cast<PCMDeferredPolyData*>(mDeferredContacts->end());
					mDeferredContacts->forceSize_Unsafe(newSize);

					data->mTriangleIndex = triangleIndex;
					data->mFeatureIndex = feature1;
					data->triFlags32 = PxU32(triFlags);
					data->mInds[0] = triIndices[0];
					data->mInds[1] = triIndices[1];
					data->mInds[2] = triIndices[2];
					V3StoreU(localTriangle.verts[0], data->mVerts[0]);
					V3StoreU(localTriangle.verts[1], data->mVerts[1]);
					V3StoreU(localTriangle.verts[2], data->mVerts[2]);
				}
				return true;
			}
		}
		else
		{
			PxI32 index2;
			feature1 = PxU32(getPolygonIndex(polyData, polyMap, minNormal, index2));
			const HullPolygonData* referencePolygon = &polyData.mPolygons[feature1];
			
			const Vec3V contactNormal = V3Normalize(M33TrnspsMulV3(polyMap->shape2Vertex, V3LoadU(referencePolygon->mPlane.n)));
			const Vec3V nContactNormal = V3Neg(contactNormal);

			//if the minimum sperating axis is edge case, we don't defer it because it is an activeEdge
			patchNormal = nContactNormal;
			generatedPolyContacts(polyData, *referencePolygon, localTriangle, triangleIndex, triFlags, polyMap, manifoldContacts, numContacts, contactDist, contactNormal, mRenderOutput);
		}
	}

	return true;
}

bool Gu::PCMConvexVsMeshContactGeneration::generateTriangleFullContactManifold(const TriangleV& localTriangle, PxU32 triangleIndex, PxU8 triFlags, const PolygonalData& polyData, const SupportLocalImpl<TriangleV>* localTriMap, const SupportLocal* polyMap, MeshPersistentContact* manifoldContacts, PxU32& numContacts,
	const FloatVArg contactDist, Vec3V& patchNormal, PxRenderOutput* renderOutput)
{
	const FloatV threshold = FLoad(0.7071f);//about 45 degree
	PX_UNUSED(threshold);
	{
		FeatureStatus status = POLYDATA0;
		FloatV minOverlap = FMax();
		//minNormal will be in the local space of polyData
		Vec3V minNormal = V3Zero();

		PxU32 feature0;
		if(!testTriangleFaceNormal(localTriangle, polyData, localTriMap, polyMap, contactDist, minOverlap, feature0, minNormal, POLYDATA0, status))
			return false;
			
		PxU32 feature1;
		if(!testPolyFaceNormal(polyData, localTriMap, polyMap, contactDist, minOverlap, feature1, minNormal, POLYDATA1, status))
			return false;

		if(!testPolyEdgeNormal(localTriangle, triFlags, polyData, localTriMap, polyMap, contactDist, minOverlap, minNormal, EDGE, status))
			return false;

		const Vec3V triNormal = localTriangle.normal();
		patchNormal = triNormal;

		const HullPolygonData* referencePolygon = &polyData.mPolygons[getPolygonIndex(polyData, polyMap, triNormal)];
		generatedTriangleContacts(localTriangle, triangleIndex, triFlags, polyData, *referencePolygon, polyMap, manifoldContacts, numContacts, contactDist, triNormal, renderOutput);
	}

	return true;
}

bool Gu::PCMConvexVsMeshContactGeneration::generatePolyDataContactManifold(const TriangleV& localTriangle, PxU32 featureIndex, PxU32 triangleIndex, PxU8 triFlags, MeshPersistentContact* manifoldContacts, PxU32& numContacts, const FloatVArg contactDist, Vec3V& patchNormal)
{
	const HullPolygonData* referencePolygon = &mPolyData.mPolygons[featureIndex];
		
	const Vec3V contactNormal = V3Normalize(M33TrnspsMulV3(mPolyMap->shape2Vertex, V3LoadU(referencePolygon->mPlane.n)));
	const Vec3V nContactNormal = V3Neg(contactNormal);
		
	patchNormal = nContactNormal;
	generatedPolyContacts(mPolyData, *referencePolygon, localTriangle, triangleIndex, triFlags, mPolyMap, manifoldContacts, numContacts, contactDist, contactNormal, mRenderOutput);

	return true;
}
